#include "autonomous_attack_algorithm/autonomous_attack.hpp"
#include <iostream>
#include <mutex>
#include <cmath>


Attack::Attack():Node("autonomous_attack_algorithm_node"){


    // 声明无人机的默认参数
    this->declare_parameter<int>("parameters.vehicle_id", 0);
    this->declare_parameter<std::string>("parameters.namesspace", "/drone_0");

    this->declare_parameter<std::string>("subscribers.trackerid_sub.topic", "/detection_image/result_box");
    this->declare_parameter<std::string>("subscribers.call_server.topic", "/drone_attack_target_server_topic_signal");
    this->declare_parameter<std::string>("subscribers.tracker_server.topic", "/drone_attack_target_id");

    this->declare_parameter<std::string>("publishers.attack_status", "/tracker_status");


    vehicle_id_ = this->get_parameter("parameters.vehicle_id").as_int();
    namespace_ = this->get_parameter("parameters.namesspace").as_string();
    yolo_topic_name = this->get_parameter("subscribers.trackerid_sub.topic").as_string();
    yolo_topic_name = namespace_ + yolo_topic_name;
    start_topic_name = this->get_parameter("subscribers.call_server.topic").as_string();
    start_topic_name = namespace_ + start_topic_name;
    attack_id_topic_name = this->get_parameter("subscribers.tracker_server.topic").as_string();
    attack_id_topic_name = namespace_ + attack_id_topic_name;
    attack_topic_name = this->get_parameter("publishers.attack_status").as_string();
    attack_topic_name = namespace_ + attack_topic_name;
    
    first_find_target = false;

    yolo_sub= this->create_subscription<yolo_detect_message::msg::DetectBoxs>(yolo_topic_name,rclcpp::QoS(10).best_effort(),std::bind(&Attack::yolo_callback,this,std::placeholders::_1));

    // 无人机进行目标的打击控制
    attack_pub = this->create_publisher<std_msgs::msg::Bool>(attack_topic_name, rclcpp::QoS(10));
    
    move_topic_name = "/uav_swarms" + namespace_ + "/move";
    move_pub_ = this->create_publisher<yolo_detect_message::msg::MoveInterface>(move_topic_name,10);
    std::string start_service_name = start_topic_name + "/service";
  
    service_ = this->create_service<yolo_detect_message::srv::Service>(start_service_name,std::bind(&Attack::service_callback,this,std::placeholders::_1,std::placeholders::_2));
    service_status_subscriber_ = this->create_subscription<yolo_detect_message::msg::CallServerStatus>(start_topic_name,rclcpp::QoS(10).best_effort(),std::bind(&Attack::service_status_callback,this,std::placeholders::_1));
    attack_id_sub_ = this->create_subscription<std_msgs::msg::Int32>(attack_id_topic_name,rclcpp::QoS(10).best_effort(),std::bind(&Attack::attack_id_callback,this,std::placeholders::_1));
    
    timer_ = this->create_wall_timer(
        std::chrono::milliseconds(50),
        std::bind(&Attack::timer_callback, this));
    echo();
}

void Attack::service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg){
  
    bool reqs = msg->status;
    service_status.store(reqs);
    
    RCLCPP_INFO(this->get_logger(),"[target tracker node] service status is %d.",service_status.load());
}




void Attack::yolo_callback(const yolo_detect_message::msg::DetectBoxs::SharedPtr msg){
    if (service_status.load() == false || msg->detect_boxes.size() == 0)
    {
        return;
    }
    
    yolo_detect_message::msg::DetectBoxs conttains_target;
    for (size_t i = 0; i < msg->detect_boxes.size(); i++)
    {
        if (msg->detect_boxes[i].id == attack_id)
        {
            conttains_target.detect_boxes.push_back(msg->detect_boxes[i]);
        }
    }
    // 如果没有的对象则返回
    if (conttains_target.detect_boxes.size() == 0){
        return;
    }
    auto compare = std::bind(&Attack::compareBydistance, this, std::placeholders::_1, std::placeholders::_2);
    std::sort(conttains_target.detect_boxes.begin(), conttains_target.detect_boxes.end(), compare);
    first_find_target = true;
    wait_time = 0;
    // 选取第一个点作为打击的对象
    auto box = conttains_target.detect_boxes[0];

    float target_x = box.xmin + box.width / 2.0;
    float target_y = box.ymin + box.heigh / 2.0;

    // 水平方向和垂直方向的误差值
    float error_x = target_location_width - target_x;
    float error_y = target_location_height - target_y;
    float velocity_x = 0.0;
    float velocity_y = 0.0;
    if (error_x > plane_error_threshold)
    {
        velocity_y = default_y_velocity;
    }else if(error_x < plane_error_threshold  * -1.0){
        velocity_y = default_y_velocity * -1.0;
    }else{
        velocity_y = 0.0;
    }
    
    if (error_y > plane_error_threshold)
    {
        velocity_x = default_x_velocity;   
    }else if(error_y < plane_error_threshold  * -1.0){
        velocity_x = default_x_velocity * -1.0;
    }else{
        velocity_x = 0.0;
    }
    set_velocity(velocity_x, velocity_y);
    if (std::abs(error_x)< plane_error_threshold && std::abs(error_y)<plane_error_threshold )
    {
        std::lock_guard<std::mutex> lock_image(is_target_in_center_mutex);
        is_target_in_center = true;
        reset_velocity();
    }
    
}


void Attack::reset_velocity(){
    std::lock_guard<std::mutex> current_velocity_mutex_lock(current_velocity_mutex);
    current_velocity.angular.set__x(0.0);
    current_velocity.angular.set__y(0.0);
    current_velocity.angular.set__z(0.0);

    current_velocity.linear.set__x(0.0);
    current_velocity.linear.set__y(0.0);
    current_velocity.linear.set__z(0.0);
}



void Attack::set_velocity(float x, float y){
    std::lock_guard<std::mutex> current_velocity_mutex_lock(current_velocity_mutex);
    current_velocity.angular.set__x(0.0);
    current_velocity.angular.set__y(0.0);
    current_velocity.angular.set__z(0.0);

    current_velocity.linear.set__x(x);
    current_velocity.linear.set__y(y);
    current_velocity.linear.set__z(0.0);
}

float Attack::clamp(float value,float max_value){
    float result= {};
    if (value > max_value)
    {
        result = max_value;
    }else if (value < (-1.0 * max_value))
    {
        result = -1.0 * max_value;
    }else{
        result = value;
    }

    return result;
    
    
}

void Attack::timer_callback(){
    if (service_status.load() == false )
        {
           RCLCPP_ERROR(this->get_logger(), "service is not ready,service_status is %s",service_status.load() ? "true":"false");
            return;
        }
        wait_time++;
        // 发布速度控制命令
        if (first_find_target == false)
        {
            reset_velocity();
        }
        if (wait_time> 25)
        {
            reset_velocity();
        }
        // 如果丢失的时间太长，则向地面站发布无人的状态丢失
        if (is_target_in_center ==  true  )
        {
            std_msgs::msg::Bool can_attack_status;
            can_attack_status.data = true;
            attack_pub->publish(can_attack_status);
        } else {
            std_msgs::msg::Bool can_attack_status;
            can_attack_status.data = false;
            attack_pub->publish(can_attack_status);
        }
        
        if ( service_status.load() == true)
        {
            yolo_detect_message::msg::MoveInterface msg;
            msg.is_body = true;
            msg.is_vel = true;
            // 使用XYZ坐标轴的控制
            msg.is_gps = false;
            msg.pose = geometry_msgs::msg::Pose();
            msg.vel = current_velocity;
            move_pub_->publish(msg);

            RCLCPP_INFO(this->get_logger(),"tracker publish velocity x= %f,y = %f",current_velocity.linear.x,current_velocity.linear.y);
        }
        RCLCPP_INFO(this->get_logger(),"control information has published");
}

void Attack::attack_id_callback(const std_msgs::msg::Int32::SharedPtr msg){
    std::lock_guard<std::mutex> lock_image(attack_id_mutex);
    attack_id = msg->data;
}


void Attack::echo(){
    // Log the parameters for confirmation
    RCLCPP_INFO(this->get_logger(), "Vehicle ID: %d", vehicle_id_);
    RCLCPP_INFO(this->get_logger(), "Namespace: %s", namespace_.c_str());
    RCLCPP_INFO(this->get_logger(), "yolo_topic_name: %s", yolo_topic_name.c_str());
    RCLCPP_INFO(this->get_logger(), "start_topic_name: %s", start_topic_name.c_str());
    RCLCPP_INFO(this->get_logger(), "attack_id_topic_name: %s", attack_id_topic_name.c_str());
    RCLCPP_INFO(this->get_logger(), "attack_topic_name: %s", attack_topic_name.c_str());
    return;

}

void Attack::service_callback(const yolo_detect_message::srv::Service::Request::SharedPtr req,
                        const yolo_detect_message::srv::Service::Response::SharedPtr res){
    bool reqs = req->status;
    if(reqs){
        RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] service callback.");
    }
    else{
        RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] service callback false.");

    }
    service_status.store(reqs);
    res->success = true;
    res->drone_statue = "yolo detect service_status is " + std::to_string(service_status);
}